/**
 * Snowflake MEGA sumo robot test nr.3 - simple see attack test fight on selected speed (30%, 50% or 100% PWM) using delays
 * Copyright 2013 Maris BECS <maris.becs@gmail.com>
 */

#include <asf.h>
#include "test3.h"
#include "keyboard.h"
#include "sensors.h"
#include "motors.h"


void test3_app(void)
{
	int8_t speed = 0;
	int8_t frc = -10;	// forward speed correction for the right motor <========== Calibrate this value!!! ===========
	struct keyboard_event input;

	gfx_mono_draw_string("ENTER - 50%, UP - 30%", 0, 14, &sysfont);
	gfx_mono_draw_string("      DOWN - 100% PWM", 0, 24, &sysfont);

	// Set the PWM
	while (true) {
		keyboard_get_key_state(&input);
		if (input.type == KEYBOARD_RELEASE) {
			if (input.keycode == KEYBOARD_ENTER) {
				speed = 50;		// ENTER - 50% PWM
				frc = -5;		// forward speed correction for right motor <========== Calibrate this value!!! ===========
				break;
			} else if (input.keycode == KEYBOARD_UP) {
				speed = 30;		// UP - 30% PWM
				frc = 0;		// forward speed correction for right motor <========== Calibrate this value!!! ===========
				break;
			} else if (input.keycode == KEYBOARD_DOWN) {
				speed = 100;	// ENTER - 100% PWM
				break;
			} else if (input.keycode == KEYBOARD_BACK) {
				// set IR module back to OFF state
				ioport_set_pin_high(START_SW);
				// go back to FIGHT menu
				return;
			}
		}
	}

	// clear screen
	gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);

	// Show some info
	gfx_mono_draw_string(" Test 3    ACTIVATED ", 0, 0, &sysfont);
	gfx_mono_draw_string(" Press ENTER/IR start", 0, 24, &sysfont);

    while (true) {
        // wait for IR remote start signal or ENTER button press
        if ( HIGH == ioport_get_pin_level(REMOTE_START) ){
            break;
        }

        keyboard_get_key_state(&input);
        if (input.type == KEYBOARD_RELEASE) {
            if (input.keycode == KEYBOARD_ENTER) {
                break;
            } else if (input.keycode == KEYBOARD_BACK) {
                // set IR module back to OFF state
                ioport_set_pin_high(START_SW);
                // go back to FIGHT menu
                return;
            }
        }
    }

    // ~5 sec delay before a start ( 950*5 = 4.75 sec )
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_high(NHD_C12832A1Z_BACKLIGHT);
        delay_ms(950);
    gpio_set_pin_low(NHD_C12832A1Z_BACKLIGHT);


	// drop the flag
	turn_servo(true);
	  //delay_ms(200);	// TODO: very stupid delay
	  //pwm_start( &pwm_cfg[1], 0 ); // to free the servo

	// drive straight with right motor correction !!!
	drive( speed, speed+frc );
	// turn on power for sensors
	sensorPower(true);

	while (true)
	{
		// check kill switch state
		if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
			// kill switch has been activated
			drive(0,0);
			// set IR module back to OFF state
			ioport_set_pin_high(START_SW);
			// turn off power for sensors
			sensorPower(false);
			pwm_start( &pwm_cfg[1], 0 ); // free the servo
			// clear screen
			gfx_mono_draw_filled_rect(0, 0, 128, 32, GFX_PIXEL_CLR);
			return;
		}

		// while loop sensor read delay
		delay_ms(3);

		// read all sensors
		Line_sens = line_read();
		Distance_sens = sensor_read();

		// drive straight with right motor correction !!!
		drive( speed, speed+frc );


		/*
		 * ######################### Check line sensor state #########################
		 */
		if( (Line_sens&0b00000010) || (Line_sens&0b00000100) ){	// Line_FL or Line_FR
			delay_us(500);
			Line_sens = line_read();
			// Line_FL
			if (Line_sens&0b00000010) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn right while NOT see line again
				while ( !(Line_sens&0b00000001) ){	// Line_BR
					Line_sens = line_read();
					drive(speed,-speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			// Line_FR
			} else if (Line_sens&0b00000100) {
				drive(0,0);	// stop
				delay_ms(50);
				// turn left while NOT see line again
				while ( !(Line_sens&0b00001000) ){	// Line_BL
					Line_sens = line_read();
					drive(-speed,speed);
					// check kill switch state
					if ( (LOW == ioport_get_pin_level(KILL_SW)) || (LOW == ioport_get_pin_level(REMOTE_KILL)) ) {
						drive(0,0);
						break;
					}
				}
			}
		}


		/*
		 * ######################### Check distance sensor state #########################
		 */
		if (Distance_sens&0b00010000) {		// L10
			drive( speed*0.7, speed );	// drive little left
			delay_ms(5);

		} else if (Distance_sens&0b00001000) {	// R10
			drive( speed, speed*0.7 );	// drive little right
			delay_ms(5);

		} else if (Distance_sens&0b00100000) {	// L30
			drive( speed/3, speed );	// drive more left
			delay_ms(5);

		} else if (Distance_sens&0b00000100) {	// R30
			drive( speed, speed/3 );	// drive more right
			delay_ms(5);

		} else if (Distance_sens&0b01000000) {	// L45
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000010) {	// R45
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);

		} else if (Distance_sens&0b10000000) {	// L90
			drive( -speed, speed );	// turn on spot left
			delay_ms(5);

		} else if (Distance_sens&0b00000001) {	// R90
			drive( speed, -speed );	// turn on spot right
			delay_ms(5);
		} else if ( HIGH == ioport_get_pin_level(PEPPERL_FUCHS) ){
			drive( speed, speed+frc );	// drive straight full speed, not sure if necessary !!!
			delay_ms(5);
		}


	}
}